#include<Algorithm/delaunay.h>
#include<Mesh/libcell_iterator.h>

#define quote lib_cell_quote

#define factorial LB_factorial

double ori_area_simplex(double ** M,int rows,int cols)
{
//由于没有使用反对称张量库，我们只能限制背景空间cols和rows的关系
    if((rows-1)!=cols)
    {
        printf("rows is not corresponse to cols\r\n");
        return 0;
    }
    LB_Matrix* mat=(LB_Matrix*)malloc(sizeof(LB_Matrix));
    lb_matrix_init_double(mat,cols,cols);
    double *data=(double*)(mat->data);
    for(int i=0;i<cols;i++)
    {
        for(int j=0;j<cols;j++)
        {
            data[i*cols+j]=M[i+1][j]-M[0][j];
        }
    }  
    double * det=(double*)(mat->det(mat));
    double re=(*det)/(double)factorial(cols);
    lb_matrix_free(mat);
    free(det);
    return re;
}

//输入cols维背景空间rows个点，输出体积数值
//rows代表点数，cols代表背景空间，比如3,7表示7维背景空间下三角形(单形))
double area_simplex(double**M,int rows,int cols)
{
    if(rows>(cols+1))
    {
        printf("points are too much\r\n");
        return 0;
    }
    LB_Matrix* A=(LB_Matrix*)malloc(sizeof(LB_Matrix));
    lb_matrix_init_double(A,rows-1,cols);
    double* data=(double*)(A->data);
    for(int i=0;i<(rows-1);i++)
    {
        for(int j=0;j<cols;j++)
        {
            data[i*cols+j]=M[i][j]-M[rows-1][j];
        }
    }
    LB_Matrix* AT=lb_matrix_get_transpose_double(A);
    A->mult(A,AT);
    double *test= (double*)(A->det(A));

    double re=(SAFE_SQRT(*test))/(double)(factorial(rows-1));
    if(isnan(re)==1)
    {
        printf("nan:%lf\n",re);
    }
    free(test);
    lb_matrix_free(A);
    lb_matrix_free(AT);
    return re; 
}

void increasing_convex_hull(template_m* m,template_v* v)
{
    int rows=v->point_size+1,cols=v->point_size;
    double **M=(double**)malloc(sizeof(double*)*rows);
    for(int i=0;i<rows;i++)
    {
        M[i]=(double*)malloc(sizeof(double)*cols);  
    }
    for(int j=0;j<cols;j++)
    {
        M[rows-1][j]=v->point[j];   
    }

    Node* node=NULL,*node2=NULL; 
// //find first
    Int_RB_Tree *tree=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tree);
    for(auto cit=m->c_begin(m);cit!=m->c_end(m);cit++)
    {
        for(int i=0;i<cols;i++)
        {
            for(int j=0;j<cols;j++)
            {   
                M[i][j]=((template_v*)(quote(cit)->vertices[i]))->point[j];        
            }
        }
        if(ori_area_simplex(M,rows,cols)<0)
        {
            tree->insert(tree,quote(cit)->id,quote(cit));
            node=node_overlying(node,quote(cit)); 
            node2=node_overlying(node2,quote(cit));
            break;
        } 
    } 
    template_c* c0=NULL,*c1=NULL;
    template_hf*hf1=NULL;
//     //广度优先

    while(node!=NULL)
    {
        Node * temp_node=node;
        node=NULL;
        for(Node* node_it=temp_node;node_it!=NULL;node_it=(Node*)(node_it->Next))
        {
            c0=(template_c*)(node_it->value);
            for(auto chf_it=m->chf_begin(m,*c0);chf_it!=m->chf_end(m,*c0);chf_it++)
            {
                hf1=m->s_opposite_halfface(quote(chf_it));
                c1=(template_c*)(hf1->cell);
                if(c1==NULL||tree->find(tree,c1->id)!=NULL)
                {continue;} 
                else
                {
                    for(int i=0;i<cols;i++)
                    {
                        for(int j=0;j<cols;j++)
                        {
                            M[i][j]=((template_v*)(c1->vertices[i]))->point[j];     
                        }
                    }
                    if(ori_area_simplex(M,rows,cols)<0)
                    {
                        node=node_overlying(node,c1);
                        node2=node_overlying(node2,c1);  
                    }
                    tree->insert(tree,c1->id,c1);
                }
            }
        }
        free_node(temp_node);
    } 
    int_rb_tree_free(tree);
    //delete cells
    for(Node* nit=node2;nit!=NULL;nit=(Node*)(nit->Next))
    {

       m->delete_cell(m,*((template_c*)(nit->value)),true); 
    }
    free_node(node2);

    // for(auto cit=m->c_begin(m);cit!=m->c_end(m);cit++)
    // {
    //     for(int j=0;j<quote(cit)->vertices_size;j++)
    //     {
    //         printf("%d ",quote(cit)->vertices[j]->id);
    //     }
    //     printf("\n");
    // }
    // printf("begin mend loopholes\n");
    //return ;
    //Mend loopholes
    //注意此处的haslfaces的顺序
    m->external_cells_init_(m);
    

    //printf("end mend loopholes\n");

    template_v** temp_v=(template_v**)malloc(sizeof(template_v)*cols);
    temp_v[cols-1]=v;

    for(Node* nit =m->external_cells;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_c* external_cell=(template_c*)(nit->value);

        //for(Node* hfit=(external_cell->halffaces);hfit!=NULL;hfit=(Node*)(hfit->Next))
        for(auto chfit=m->chf_begin(m,*external_cell);chfit!=m->chf_end(m,*external_cell);chfit++)
        {          
       // printf("hreeelibo\n");
            for(int i=0;i<quote(chfit)->vertices_size;i++)
            {
                temp_v[i]=quote(chfit)->vertices[i];
            }
            m->create_cellv(m,temp_v,cols); 
        }
    }
    
    for(int i=0;i<rows;i++)
    {
        free(M[i]);
    }
    free(M);
    free(temp_v);
}

//mesh是只包含顶点集合的mesh
void mesh_createconvex(template_m*m)
{
    if(m->num_v(m)<=0)
    {
        return;
    }
    auto iter=m->v_begin(m);
    m->simplex=1;m->dimension=quote(iter)->point_size-1;
    int rows=m->dimension+2,cols=m->dimension+1;
    double **VV=(double**)malloc(sizeof(double*)*(rows));
    for(int i=0;i<rows;i++)
    {
        VV[i]=(double*)malloc(sizeof(double)*(cols));
    }
    template_v** temp_v=(template_v**)malloc(sizeof(template_v*)*rows); 
    for(int i=0;i<rows;i++)
    {
        for(int j=0;j<cols;j++)
        {
             VV[i][j]=quote(iter)->point[j];
        }
        temp_v[i]=quote(iter);
        iter++;
    }
    int *index=(int*)malloc(sizeof(int)*(rows));
    for(int i=0;i<rows;i++)
    {
        index[i]=i;
    }
    if(ori_area_simplex(VV,rows,cols)<0)
    {
        index[0]=1;
        index[1]=0;   
    }
    template_v **temp_v1=(template_v**)malloc(sizeof(template_v*)*cols);
    template_v*v0=NULL;
    for(int i=0;i<rows;i++)
    {
        int k=0;
        for(int j=0;j<rows;j++)
        {
            if(j!=i)
            {
                temp_v1[k]=temp_v[index[j]];
                k++;
            }

        }
        if((cols-i)%2!=0)
        {
            v0=temp_v1[0];
            temp_v1[0]=temp_v1[1];
            temp_v1[1]=v0;
        }
        m->create_cellv(m,temp_v1,cols);
    }
    free(index);
    for(int i=0;i<rows;i++)
    {
        free(VV[i]);
    }
    free(VV);
    for(;iter!=m->v_end(m);iter++)
    {
        increasing_convex_hull(m,quote(iter)); 
    }
    free(temp_v);free(temp_v1);
}
//给一个包含顶点数据的数组，会得到凸包网格
void  from_v_createconvex(template_m* mesh,double** VV,int rows,int cols)
{
    if(rows<cols+1)
    {
        return;
    }
    mesh->simplex=1;mesh->manifold_require=1;mesh->dimension=cols-1;
    for(int i=0;i<cols+1;i++)
    {
        mesh->create_vertexv(mesh,VV[i],cols);
    }
    int *index=(int*)malloc(sizeof(int)*(cols+1));
    for(int i=0;i<cols+1;i++)
    {
        index[i]=i;
    }
    if(ori_area_simplex(VV,cols+1,cols)<0)
    {
        index[0]=1;
        index[1]=0;   
    }
    template_v **temp_v=(template_v**)malloc(sizeof(template_v*)*cols);

    for(int i=0;i<cols+1;i++)
    {
        int k=0;
        for(int j=0;j<cols+1;j++)
        {
            if(j!=i)
            {
                temp_v[k]=mesh->get_vertexp(mesh,index[j]);
                k++;
            }
        }
        if((cols-i)%2!=0)
        {
            template_v* temp_v1=temp_v[0];
            temp_v[0]=temp_v[1];
            temp_v[1]=temp_v1;
        }
        mesh->create_cellv(mesh,temp_v,cols);
    }
    free(temp_v);
    free(index);
    //mesh->printself(mesh);
    for(int i=cols+1;i<rows;i++)
    {
        template_v* v=mesh->create_vertexv(mesh,VV[i],cols);
        increasing_convex_hull(mesh,v);
        //mesh->printself(mesh);
    }
}

void delaunay_triangulation_from_mesh(template_m* m)
{
    if(m==NULL){return;}

    if(m->dimension==quote(m->v_begin(m))->point_size)
    {
        for(auto it=m->v_begin(m);it!=m->v_end(m);it++)
        {
            double* point=(double*)malloc(sizeof(double)*(m->dimension+1));
            double value=0;
            for(int i=0;i<m->dimension;i++)
            {
                value+=(quote(it)->point[i]*quote(it)->point[i]);
                point[i]=quote(it)->point[i];
            }
            point[m->dimension]=value;
            free(quote(it)->point);
            quote(it)->point=point; 
            quote(it)->point_size=m->dimension+1;
        }   
    }
    else
    {
        for(auto it=m->v_begin(m);it!=m->v_end(m);it++)
        {
            double value=0;
            for(int i=0;i<m->dimension;i++)
            {
                value+=(quote(it)->point[i]*quote(it)->point[i]);
            }
            quote(it)->point[m->dimension]=value;
        }
    }
    printf("end one\n");

    auto it=m->v_begin(m); 
    double *point=(double*)malloc(sizeof(double)*(m->dimension+1));
    for(int i=0;i<m->dimension+1;i++)
    {
        point[i]=quote(it)->point[i];
    }
    point[m->dimension]*=2.0;
    point[m->dimension]+=9000.0;
    template_v* v=m->create_vertexv(m,point,m->dimension+1); 
    free(point);
    printf("insert one\n");
//*****************************
    int rows=m->dimension+2,cols=m->dimension+1;
    double **VV=(double**)malloc(sizeof(double*)*(rows));
    for(int i=0;i<rows;i++)
    {
        VV[i]=(double*)malloc(sizeof(double)*(cols));
    }
    int *index=(int*)malloc(sizeof(int)*(rows));
    template_v** temp_v=(template_v**)malloc(sizeof(template_v*)*rows); 
    for(int i=0;i<rows-1;i++)
    {
        for(int j=0;j<cols;j++)
        {
             VV[i][j]=quote(it)->point[j];
        }
        temp_v[i]=quote(it);
        index[i]=i;
        it++;
    }
    temp_v[rows-1]=v;
    for(int j=0;j<cols;j++)
    {
        VV[rows-1][j]=v->point[j];
    }
    index[rows-1]=rows-1;

    if(ori_area_simplex(VV,rows,cols)<0)
    {
        index[0]=1;
        index[1]=0;   
    }
    template_v **temp_v1=(template_v**)malloc(sizeof(template_v*)*cols);
    template_v*v0=NULL;
    for(int i=0;i<rows;i++)
    {
        int k=0;
        for(int j=0;j<rows;j++)
        {
            if(j!=i)
            {
                temp_v1[k]=temp_v[index[j]];
                k++;
            }
        }
        if((cols-i)%2!=0)
        {
            v0=temp_v1[0];
            temp_v1[0]=temp_v1[1];
            temp_v1[1]=v0;
        }
        m->create_cellv(m,temp_v1,cols);
    }
    free(temp_v1);
    free(index);
    for(int i=0;i<rows;i++)
    {
        free(VV[i]);
    }
    free(VV);
    free(temp_v);
    printf("insert one cell %d\n",m->num_c(m));

    //increasing_convex_hull(m,quote(it)); 
    int i=0;
    for(;it!=m->v_end(m)&&quote(it)!=v;it++)
    {
        printf("increasint :%d\n",i);
        increasing_convex_hull(m,quote(it));
        i++; 
    } 
    Node* node_c=NULL;
    for(auto vcit=m->vc_begin(m,*v);vcit!=m->vc_end(m,*v);vcit++ )
    {
        node_c=node_overlying(node_c,quote(vcit));
    }
    for(Node* nit=node_c;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_c* c=(template_c*)(nit->value);
        m->delete_cell(m,*c,true);
    }

    free_node(node_c);
//*******************
}